import edu.wsu.KheperaSimulator.KSGripperStates;

/**
 * 
 * @author Nixxx This Controller first searches for a ball, and if found uses
 *         his gripper to grab and lift it. Then it searches for the next ball
 *         to push it to the opposite wall. If a ball is present in the gripper,
 *         it also gets released at a wall.
 */

public class BallGripperWallPusher extends Subsumption {

	/**
	 * Flag for checking if ball should be gripped
	 */
	private boolean m_bgripperFlag;

	/**
	 * String that tells if to grip or release a ball, or do no action
	 */
	private String m_gripperAction;
	/**
	 * Enum for remembering robot state
	 */
	private State m_state;
	/**
	 * integer array for remembering motor speeds of ball gripper behaviour
	 */
	private int[] m_bgripperOutput = { 0, 0 };


	public BallGripperWallPusher() {
		setWaitTime(7);
		m_cruiserFlag = false;
		m_wcheckerFlag = false;
		m_bfinderFlag = false;
		m_bpusherFlag = false;

		m_gripperAction = "no";

		m_state = State.cruise;

		m_milage = 0;
	}


	public void doWork() throws Exception {
		wallChecker();

		if (m_wcheckerFlag == false) {
			ballGripper();

			if (m_bgripperFlag == false) {
				ballPusher();

				if (m_bpusherFlag == false) {
					ballFinder();

					if (m_bfinderFlag == false) {
						cruise();
					}

				}

			}

		}

		arbitrate();
		setMilage();
	}


	public void arbitrate() {

		if (m_cruiserFlag == true) {
			System.out.println("Cruise");
			m_state = State.cruise;
			m_gripperAction = "no";
		}
		if (m_bfinderFlag == true) {
			System.out.println("Ballfinder");
			m_state = State.find;
			m_gripperAction = "no";
		}
		if (m_bpusherFlag == true) {
			System.out.println("Ballpusher");
			m_state = State.push;
			m_gripperAction = "no";
		}
		if (m_bgripperFlag == true) {
			System.out.println("Ballgripper");
			m_state = State.grip;
			m_gripperAction = "grip";
		}
		if (m_wcheckerFlag == true) {
			System.out.println("wallChecker");
			m_state = State.check;
			m_gripperAction = "release";
		}

		doGripperAction();

		setMotorSpeed();
		// giving the robot time to do a proper escape move
		if (m_wcheckerFlag == true) {
			this.sleep(1000);
		}
	}


	/**
	 * Method for remembering if a ball should be grabbed or not
	 */
	public void ballGripper() {
		getSensorValues();
		// if ball is in front and nowhere else and robot has no ball in gripper
		if ((s2 >= SENSOR_MAX || s3 >= SENSOR_MAX) && (!this.isObjectHeld())
				&& (m_wcheckerFlag == false) && nothingLeft() && nothingRight()) {

			m_bgripperOutput[0] = 8;
			m_bgripperOutput[1] = 8;
			m_bgripperFlag = true;
		} else {
			m_bgripperFlag = false;
		}
	}


	/**
	 * This method sets the motor speeds according to the highest priority
	 * state. This has to be done because the gripper action takes some time and
	 * setting the motor speeds can only be done after completing the gripper
	 * tasks.
	 */
	public void setMotorSpeed() {

		if (m_state == State.cruise) {
			setMotorSpeeds(m_cruiseOutput[0], m_cruiseOutput[1]);
		} else if (m_state == State.find) {
			setMotorSpeeds(m_bfinderOutput[0], m_bfinderOutput[1]);
		} else if (m_state == State.push) {
			setMotorSpeeds(m_bgripperOutput[0], m_bgripperOutput[1]);
		} else if (m_state == State.grip) {
			setMotorSpeeds(m_bgripperOutput[0], m_bgripperOutput[1]);
		} else if (m_state == State.check) {
			setMotorSpeeds(m_wcheckerOutput[0], m_wcheckerOutput[1]);
		}

	}


	/**
	 * This method stops the motors and carries out the specific gripper action
	 */
	public void doGripperAction() {

		if (m_gripperAction == "grip") {

			setMotorSpeeds(0, 0);
			setArmState(KSGripperStates.ARM_DOWN);
			sleep(500);
			setGripperState(KSGripperStates.GRIP_CLOSED);
			sleep(500);
			setArmState(KSGripperStates.ARM_UP);
			sleep(500);
		} else if (m_gripperAction == "release") {

			setMotorSpeeds(0, 0);
			if (getGripperState() == KSGripperStates.GRIP_CLOSED) {
				setGripperState(KSGripperStates.GRIP_OPEN);
				sleep(500);
			}
			if (getArmState() == KSGripperStates.ARM_DOWN) {
				setArmState(KSGripperStates.ARM_UP);
				sleep(500);
			}
		}
	}
}
